#include <public/capathtracker_iface_v1.h>

// ***********************************************************************

// class EDROOMTopContext

// ***********************************************************************



	// CONSTRUCTORES***********************************************

CAPathTracker::EDROOMTopContext::EDROOMTopContext (CAPathTracker &act, CDPath & EDROOMpVarVCurrentPath, TEDROOMUInt8 & EDROOMpVarVCurrentPathStep, TEDROOMUInt8 & EDROOMpVarVSimError ):

	EDROOMcomponent(act)
	,Msg(EDROOMcomponent.Msg)
	,MsgBack(EDROOMcomponent.MsgBack)
	,TrackPath(EDROOMcomponent.TrackPath)
	,PathTrackerCtrl(EDROOMcomponent.PathTrackerCtrl)
	,Timer(EDROOMcomponent.Timer)
	, VCurrentPath ( EDROOMpVarVCurrentPath )
	, VCurrentPathStep ( EDROOMpVarVCurrentPathStep )
	, VSimError ( EDROOMpVarVSimError )
 {

}

CAPathTracker::EDROOMTopContext::EDROOMTopContext ( EDROOMTopContext &contex ):

	EDROOMcomponent(contex.EDROOMcomponent),
	Msg(contex.Msg),
	MsgBack(contex.MsgBack)
	,TrackPath(contex.TrackPath)
	,PathTrackerCtrl(contex.PathTrackerCtrl)
	,Timer(contex.Timer)
	, VCurrentPath ( contex.VCurrentPath )
	, VCurrentPathStep ( contex.VCurrentPathStep )
	, VSimError ( contex.VSimError )
 {

}

	// EDROOMSearchContextTrans********************************************

bool CAPathTracker::EDROOMTopContext::EDROOMSearchContextTrans(TEDROOMTransId &edroomCurrentTrans){

	bool edroomValidMsg=false; 

	 switch(Msg->signal){

		 case ( EDROOMSignalDestroy ): 

			 edroomValidMsg=true;
			 edroomCurrentTrans.distanceToContext = 0 ;
			 edroomCurrentTrans.localId = -1 ;
			 break;

	}

	return(edroomValidMsg);

}

	// Component Methods  ****************************

void	CAPathTracker::EDROOMTopContext::FGoToPathStep()

{
 
 print_char('\n');
 print_char('\r');
 print_char('+');
 print_uint8(VCurrentPathStep);
 actuators_go_to_location(VCurrentPath.pathLocations[VCurrentPathStep]);
 
 VCurrentPathStep++;
 
}



void	CAPathTracker::EDROOMTopContext::FNewPath()

{
	VCurrentPath = *(CDPath *)Msg->data;
 
 	VCurrentPathStep=0;
 
 
}



void	CAPathTracker::EDROOMTopContext::FNoError()

{
 
		// reply 
	
	Msg->reply(SConfigOK);
 
 
 
}



void	CAPathTracker::EDROOMTopContext::FProgTrackStep()

{
 
		// Timing Service 
	 
	Pr_Time	interval(0.4); // Declare time var with an initial value in seconds
	 
	Timer.InformIn( interval, EDROOMprioHigh );
 
 
 
 
}



void	CAPathTracker::EDROOMTopContext::FRequestPath()

{
 
		// send new Msg
	
	TrackPath.send(SPathRequest, EDROOMprioHigh);
	
 
 
 
}



void	CAPathTracker::EDROOMTopContext::FSimError()

{
 
  VSimError--;
 
		// reply 
	
	Msg->reply(SConfigError);
 
 
 
}



TEDROOMBool	CAPathTracker::EDROOMTopContext::GLastStep()

{
 
	return (VCurrentPathStep>=VCurrentPath.pathSteps);
 
 
 
 
}



	//*****Pools



// ***********************************************************************

// class EDROOMTopState

// ***********************************************************************



	// CONSTRUCTOR*************************************************

CAPathTracker::EDROOMTopState::EDROOMTopState (CAPathTracker &act ):
	  EDROOMTopContext( act 
		, VCurrentPath
		, VCurrentPathStep
		, VSimError )
	, VCurrentPathStep ( 0 )
	, VSimError ( 2 )
{

}

	// EDROOMBehaviour**********************************************

void CAPathTracker::EDROOMTopState::EDROOMBehaviour(){

	TEDROOMTransId edroomCurrentTrans;

	edroomCurrentTrans = EDROOMIllegada();

	do{

		switch(edroomCurrentTrans.localId){

			case(Init):
				edroomNextState = Idle;
				break;
			case(NewPath):
				FNewPath();
				edroomNextState = TrackingPath;
				break;
			case(PathStep):
				FGoToPathStep();

				EDROOMcomponent.EDROOMTraceStateEntry();

				if( GLastStep() ){
					FRequestPath();
					edroomCurrentTrans.localId = PathStep_LastStep;
					edroomNextState = StandBy;
				 } 
				else{
					edroomCurrentTrans.localId = PathStep_toNextStep;
					edroomNextState = TrackingPath;
				 } 
				EDROOMcomponent.EDROOMTraceTrans(edroomCurrentTrans, Msg->signal);
				break;
			case(StopTracking):
				edroomNextState = StandBy;
				break;
			case(Config):

				EDROOMcomponent.EDROOMTraceStateEntry();

				if( VSimError ){
					FSimError();
					edroomCurrentTrans.localId = Config_Error;
					edroomNextState = Idle;
				 } 
				else{
					FNoError();
					edroomCurrentTrans.localId = Config_OK;
					edroomNextState = Idle;
				 } 
				EDROOMcomponent.EDROOMTraceTrans(edroomCurrentTrans, Msg->signal);
				break;
			case(Start):
				edroomNextState = StandBy;
				break;
		}

		EDROOMcomponent.EDROOMTraceStateEntry();

		EDROOMcomponent.EDROOMTraceStateEntry();

		switch(edroomNextState){

			case(I):
				edroomCurrentTrans=EDROOMIllegada();
				break;

			case(Idle):
				edroomCurrentTrans=EDROOMIdlellegada();
				break;

			case(StandBy):
				edroomCurrentTrans=EDROOMStandByllegada();
				break;

			case(TrackingPath):
				FProgTrackStep();
				edroomCurrentTrans=EDROOMTrackingPathllegada();
				break;

		}

		EDROOMcomponent.EDROOMTraceStateExit();

		edroomCurrentState=edroomNextState;

	}while(Msg->signal != EDROOMSignalDestroy);

}



	// Inicio**********************************************

void CAPathTracker::EDROOMTopState::EDROOMInicio(){

edroomCurrentState=I;

}



//	 ***********************************************************************

//	 llegada SubEstado I

//	 ***********************************************************************



TEDROOMTransId CAPathTracker::EDROOMTopState::EDROOMIllegada(){

	TEDROOMTransId edroomCurrentTrans;

	edroomCurrentTrans.localId = Init;
	edroomCurrentTrans.distanceToContext = 0;
	EDROOMcomponent.EDROOMTraceTrans(edroomCurrentTrans, 0);

	return(edroomCurrentTrans);

}



	// ***********************************************************************

	// llegada SubEstado Hoja Idle

	// ***********************************************************************



TEDROOMTransId CAPathTracker::EDROOMTopState::EDROOMIdlellegada(){

	TEDROOMTransId edroomCurrentTrans;

	int edroomValidMsg=false;

	do{

		EDROOMNuevoMens ();

		switch(Msg->signal){

			case (SConfigPathTracker): 

				 if ( *Msg->GetPInterface() == PathTrackerCtrl) {

					edroomCurrentTrans.localId = Config;
					edroomCurrentTrans.distanceToContext = 0 ;
					edroomValidMsg=true;
				 }

				break;

			case (SStartTracking): 

				 if ( *Msg->GetPInterface() == PathTrackerCtrl) {

					edroomCurrentTrans.localId= Start;
					edroomCurrentTrans.distanceToContext = 0;
					edroomValidMsg=true;
				 }

				break;

		};

		if (!edroomValidMsg) edroomValidMsg = EDROOMSearchContextTrans(edroomCurrentTrans);

	} while (!edroomValidMsg);

	EDROOMcomponent.EDROOMTraceTrans(edroomCurrentTrans, Msg->signal);

	return(edroomCurrentTrans);

}



	// ***********************************************************************

	// llegada SubEstado Hoja StandBy

	// ***********************************************************************



TEDROOMTransId CAPathTracker::EDROOMTopState::EDROOMStandByllegada(){

	TEDROOMTransId edroomCurrentTrans;

	int edroomValidMsg=false;

	do{

		EDROOMNuevoMens ();

		switch(Msg->signal){

			case (STrackPath): 

				 if ( *Msg->GetPInterface() == TrackPath) {

					edroomCurrentTrans.localId= NewPath;
					edroomCurrentTrans.distanceToContext = 0;
					edroomValidMsg=true;
				 }

				break;

		};

		if (!edroomValidMsg) edroomValidMsg = EDROOMSearchContextTrans(edroomCurrentTrans);

	} while (!edroomValidMsg);

	EDROOMcomponent.EDROOMTraceTrans(edroomCurrentTrans, Msg->signal);

	return(edroomCurrentTrans);

}



	// ***********************************************************************

	// llegada SubEstado Hoja TrackingPath

	// ***********************************************************************



TEDROOMTransId CAPathTracker::EDROOMTopState::EDROOMTrackingPathllegada(){

	TEDROOMTransId edroomCurrentTrans;

	int edroomValidMsg=false;

	do{

		EDROOMNuevoMens ();

		switch(Msg->signal){

			case (EDROOMSignalTimeout): 

				 if ( *Msg->GetPInterface() == Timer) {

					edroomCurrentTrans.localId = PathStep;
					edroomCurrentTrans.distanceToContext = 0 ;
					edroomValidMsg=true;
				 }

				break;

			case (SStopTracking): 

				 if ( *Msg->GetPInterface() == PathTrackerCtrl) {

					edroomCurrentTrans.localId= StopTracking;
					edroomCurrentTrans.distanceToContext = 0;
					edroomValidMsg=true;
				 }

				break;

		};

		if (!edroomValidMsg) edroomValidMsg = EDROOMSearchContextTrans(edroomCurrentTrans);

	} while (!edroomValidMsg);

	EDROOMcomponent.EDROOMTraceTrans(edroomCurrentTrans, Msg->signal);

	return(edroomCurrentTrans);

}



